#ifndef GTD_TRACKER_HPP
#define GTD_TRACKER_HPP
#include "gtd_common.hpp"

NAMESPACE_BEG(gtd)

class FeatureTracker
{
public:
    enum ImageType
    {
        TEMP,
        INTENSITY,
        GRADX,
        GRADY,
        IMAGE_TYPE_COUNT
    };

    FeatureTracker(
        uint width, 
        uint height, 
        uint targetCount = 100,
        uint minDist = 10, 
        uint levels = 2, 
        uint subsample = 4);
    ~FeatureTracker();


    void update(std::vector<FeaturePair>& pairs, const uchar4* d_frame);
    const std::vector<float*>& getPyramid(ImageType type) const;
    const std::vector<uint2>& getDims() const;

private:
    void preprocess(const uchar4* d_frame);
    void trackOldFeatures();
    void selectNewFeatures();

    // configuration properties
    const uint width;
    const uint height;
    const uint npixels;
    const uint targetCount;
    const uint minDist;
    const uint levels;
    const uint subsample;

    // CPU values
    std::vector<float> eigenvalues;

    struct EigenPoint
    {
        float value;
        ushort2 pos;

        bool operator <(const EigenPoint& rhs)
        {
            return value < rhs.value;
        }
    };

    std::vector<EigenPoint> eigenpoints;
    std::vector<Uint32> distMap;
    std::vector<FeaturePair> features;

    // tracking stats
    uint lostMinDet;
    uint lostMaxIts;
    uint lostMaxRes;

    // gaussian convolution kernels
    std::vector<float> smoothKernels[2];
    std::vector<float> pyramidKernels[2];
    std::vector<float> gradientKernels[2];

    // GPU memory pointers
    std::vector<size_t> imageSizes;
    std::vector<uint2> imageDims;
    std::vector<float*> images[IMAGE_TYPE_COUNT][2];
    float* d_eigenvalues;
    FeaturePair* d_features;

    // frame swapping indices
    int old;
    int cur;
};

void downsample(float* dest, const uint2& destDims, const float* src, 
                const uint2& srcDims);
void normalizeImage(float* dest, const uchar4* src, const uint2& dims);
void minEigenvalue(float* dest, const float* gradx, const float* grady, 
                   const uint2& dims);
void trackFeatures(FeaturePair* pairs, const uint count,
                   const float* image1, const float* image2, 
                   const float* gradx1, const float* gradx2,
                   const float* grady1, const float* grady2,
                   const uint2& dims,
                   const float subsample);

NAMESPACE_END(gtd)

#endif
